#ifndef __COMPONENT_HH__
#define __COMPONENT_HH__
#include <vector>
#include "aux.hh"
#include "matrix_tool.hh"

enum EngType { X = 0,
               Y,
               Z,
               YZ };

enum ActDynType { FIRST = 0,
                  NO_DYN };

class ACT
{
public:
    ACT(){};
    ~ACT(){};

    double ActOuptut;     /* *o  (rad) Actuator output angle or can be treated as
                           length */
    double ActRateOutput; /* *o (r/s) Actuator output rate */
    double ActAccOutput;  /* *o  (r/s2)  Actuator output acceleration */

    virtual void Actuate(double input_command, double int_step) = 0;
};  // ACT

class ACT_1st : public ACT
{
    TRICK_INTERFACE(ACT_1st);

public:
    ACT_1st(){};
    ~ACT_1st(){};

    virtual void Actuate(double input_command, double int_step);
    void set_1st_act_var(double Ang_limit_in, double Ang_rate_limit_in,
                         double Ang_acc_limit_in, double Tau_in);

protected:
    /* Variables declaration */
    double Ang_limit;       /* *o (r) Actuator's maximum deflection angle */
    double Ang_rate_limit;  /* *o (r/s) Actuator's maximum angular rate */
    double Ang_acc_limit;   /* *o (r/s2) Actuator's maximum angular acceleration */
    double Tau;             /* *o (--) Actuator's time constant */
    double Act_prior_state; /* *o (r) Actuator's prior state */
    double Act_prior_rate;  /* *o (r/s) Actuator's proir angular rate */
};

class ACT_NO_DYN : public ACT
{
    TRICK_INTERFACE(ACT_NO_DYN);

public:
    ACT_NO_DYN(){};
    ~ACT_NO_DYN(){};
    virtual void Actuate(double input_command, double int_step);
};

class ENG
{
    TRICK_INTERFACE(ENG);

public:
    ENG();
    ~ENG(){};

    /* Variables declaration */
    VECTOR(Q, 6);        /* *o (--)  External force produced by the engine */
    MATRIX(T_N_B, 3, 3); /* *o  (--)  T.M. from vehicle body to Nozzle */
    std::vector<ACT *> Act_list;
    enum EngType type;  // Engine type
    VECTOR(F, 3);       /* *o (--) TVC Force */
    VECTOR(M, 3);       /* *o (--) TVC moment */

    /* Function declaration */
    void calculate_Q(double input_ang, double thrust_in, arma::mat33 TBI,
                     double rp_in, enum EngType TYPE);
    void calculate_Q(double input_ang_1, double input_ang_2, double thrust_in,
                     arma::mat33 TBI, double rp_in, enum EngType TYPE);
    void Allocate_Actuator(int NumAct, enum ActDynType TYPE);
    void set_ENG_HINGE_POS(double x, double y, double z);
    void set_ENG_Dir(int type_in);

private:
    /* Variable declaration */
    VECTOR(THRUST, 3);        /* *o (N)  Thrust force generated by the vehicle engine */
    VECTOR(ENG_HINGE_POS, 3); /* *o (--)  Engine hinge position */
    VECTOR(ENG_GAMMA_1, 3);   /* *o (--)  1st DoF Engine velocity coefficient, each
                         DoF have 3 elements */
    VECTOR(ENG_GAMMA_2, 3);   /* *o (--)  2nd DoF Engine velocity coefficient, each
                         DoF have 3 elements */
    VECTOR(ENG_GAMMA_3, 3);   /* *o (--)  3rd DoF Engine velocity coefficient, each
                         DoF have 3 elements */
    VECTOR(ENG_BETA_4, 3);    /* *o (--) 4th DoF Engine angular velocity coefficient,
                        each DoF have 3 elements */
    VECTOR(ENG_BETA_5, 3);    /* *o (--) 5th DoF Engine angular velocity coefficient,
                        each DoF have 3 elements */
    VECTOR(ENG_BETA_6, 3);    /* *o (--) 6th DoF Engine angular velocity coefficient,
                        each DoF have 3 elements */
};

class RCS_Thruster
{
    TRICK_INTERFACE(RCS_Thruster);

public:
    RCS_Thruster();
    RCS_Thruster(const RCS_Thruster &other);

    RCS_Thruster &operator=(const RCS_Thruster &other);
    enum EngType mode; /* ** (--)     Thruster actuator direction */

    void calculate_Q(double input, arma::mat33 TBI, enum EngType type_in);
    void calculate_Torque_Q(double input, arma::mat33 TBI, enum EngType type_in);
    void set_thruster_var(double in1, double in2, double in3, int in4);
    void set_RHO(double in1, double in2, double in3);
    void set_mom_max(double in1, double in2, double in3);

    VECTOR(Q, 6); /* ** (--)  Q generated by thruster */
private:
    int Schmitt_trigger(double in);
    void clear();

    double dead_zone;     /* *o  (--)    Dead zone of Schmitt trigger */
    double hysteresis;    /* *o  (--)    Hysteresis of Schmitt trigger */
    double saved_value;   /* *o  (--)    Saved Value */
    double thrust;        /* *o  (N)     Thrust */
    double Roll_mom_max;  /* *o  (N*m)  Roll moment */
    double Pitch_mom_max; /* *o  (N*m)  Pitch moment */
    double Yaw_mom_max;   /* *o   (N*m)  Yaw moment */

    VECTOR(RHO,
           3);                /* *o  (--)    Thruster location vector from reference point */
    VECTOR(RCS_GAMMA_1, 3);   /* *o (--)  1st DoF RCS velocity coefficient, each
                       DoF have 3 elements */
    VECTOR(RCS_GAMMA_2, 3);   /* *o (--)  2nd DoF RCS velocity coefficient, each
                       DoF have 3 elements */
    VECTOR(RCS_GAMMA_3, 3);   /* *o (--)  3rd DoF RCS velocity coefficient, each
                       DoF have 3 elements */
    VECTOR(RCS_BETA_4, 3);    /* *o (--) 4th DoF RCS angular velocity coefficient,
                       each DoF have 3 elements */
    VECTOR(RCS_BETA_5, 3);    /* *o (--) 5th DoF RCS angular velocity coefficient,
                       each DoF have 3 elements */
    VECTOR(RCS_BETA_6, 3);    /* *o (--) 6th DoF RCS angular velocity coefficient,
                       each DoF have 3 elements */
    VECTOR(Thrust_torque, 3); /* *o (--) RCS torque */
};
#endif  // __COMPONENT_HH__